
function dxx = Bcr4bp_StmSB1Rot2D(t , xx , aux)
%
% BCR4BP动力学模型，状态+STM，SB1Rot，2D
%
% 输入：
% t:            [1x1]            时间(s)
% xx:          [20x1]            状态(km , km/s)
% aux:        [1x1]            引力常数
%
% 输出：
% dxx:        [20x1]            动力学方程(km , km/s^2)
%
% 参考：
% [1] 2022_Boudad
%
% 时间：2024年06月03日
% 作者：张晨
% 单位：CSU
% 邮箱：chenzhang@csu.ac.cn
%%%%%%%%%%%%%%%%%%%%%%%

% ---------------- 载入参数 ----------------
mu = aux.EMRot.mu;
mus = aux.EMRot.mus;
as = aux.EMRot.as;
wm = aux.SB1Rot.wm;

% ---------------- 提取变量 ----------------
x = xx(1);
y = xx(2);
vx = xx(3);
vy = xx(4);

% ---------------- 天体位置 ----------------
% 地月系统相对SB1夹角
thetam = t * wm;

% B1位置
xb1 = mus / (mus + 1);
yb1 = 0;

% B2位置
xb2 = 0;
yb2 = 0;

% 地球位置
xe = xb1 - mu / as * cos(thetam);
ye = yb1 - mu / as * sin(thetam);

% 月球位置
xm = xb1 + (1 - mu) / as * cos(thetam);
ym = yb1 + (1 - mu) / as * sin(thetam);

% 太阳位置
xs = - 1 / (mus + 1);
ys = 0;

% % --------------- E/M 碰撞检测 ---------------
% if aux.impact_IO == 1
%     
%     % if sqrt((x - xe)^2 + (y - ye)^2) < aux.planet.req(3) / aux.dim.l_SB1
%     if sqrt((x - xe)^2 + (y - ye)^2) < 6378.1363 / 149597894
%         dxx = [];
%         return;
%     end
%     
%     % if sqrt((x - xm)^2 + (y - ym)^2) < aux.planet.req(10) / aux.dim.l_SB1
%     if sqrt((x - xm)^2 + (y - ym)^2) < 1738 / 149597894
%         dxx = [];
%         return;
%     end
%     
% end

% ----------------- SB1rot动力学方程 ----------------
r_ec_3pow = ((x - xe)^2 + (y - ye)^2)^(1.5);
r_mc_3pow = ((x - xm)^2 + (y - ym)^2)^(1.5);
r_sc_3pow = ((x - xs)^2 + (y - ys)^2)^(1.5);

r_ec_5pow = r_ec_3pow^(5/3);
r_mc_5pow = r_mc_3pow^(5/3);
r_sc_5pow = r_sc_3pow^(5/3);

% -----------------------------------------------------------
dax_dx = - (1 - mu) / (mus + 1) * (1 / r_ec_3pow - 3 * (x - xe) * (x - xe) / r_ec_5pow) ...
                        - mu / (mus + 1) * (1 / r_mc_3pow - 3 * (x - xm) * (x - xm) / r_mc_5pow) ...
                      - mus / (mus + 1) * (1 / r_sc_3pow - 3 * (x - xs) * (x - xs) / r_sc_5pow) + 1;

dax_dy = - (1 - mu) / (mus + 1) * ( - 3 * (x - xe) * (y - ye) / r_ec_5pow) ...
                        - mu / (mus + 1) * ( - 3 * (x - xm) * (y - ym) / r_mc_5pow) ...
                      - mus / (mus + 1) * ( - 3 * (x - xs) * (y - ys) / r_sc_5pow);

% day_dx = - (1 - mu) / (mus + 1) * ( - 3 * (y - ye) * (x - xe) / r_ec_5pow) ...
%                         - mu / (mus + 1) * ( - 3 * (y - ym) * (x - xm) / r_mc_5pow) ...
%                       - mus / (mus + 1) * ( - 3 * (y - ys) * (x - xs) / r_sc_5pow);
day_dx = dax_dy;

day_dy = - (1 - mu) / (mus + 1) * (1 / r_ec_3pow - 3 * (y - ye) * (y - ye) / r_ec_5pow) ...
                        - mu / (mus + 1) * (1 / r_mc_3pow - 3 * (y - ym) * (y - ym) / r_mc_5pow) ...
                      - mus / (mus + 1) * (1 / r_sc_3pow - 3 * (y - ys) * (y - ys) / r_sc_5pow) + 1;

Df = [0 , 0 , 1 , 0;
         0 , 0 , 0 , 1;
    dax_dx , dax_dy ,  0 , 2;
    day_dx , day_dy , -2 , 0];

Phi = reshape(xx(5 : 20) , 4 , 4);
dPhi = Df * Phi;

dxx = zeros(20 , 1);
dxx(1 : 4) = [vx;
    vy;
    x + 2 * vy - (1 - mu) / (mus + 1) * (x - xe) / r_ec_3pow - mu / (mus + 1) * (x - xm) / r_mc_3pow - mus / (mus + 1) * (x - xs) / r_sc_3pow;
    y - 2 * vx - (1 - mu) / (mus + 1) * (y - ye) / r_ec_3pow - mu / (mus + 1) * (y - ym) / r_mc_3pow - mus / (mus + 1) * (y - ys) / r_sc_3pow];
dxx(5 : 20) = dPhi(:);

end
